/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef VIRTUALLINEBIAS_H
#define VIRTUALLINEBIAS_H
#include "MacroDefine/GeneralDefine.h"
#include "vehicledefine.h"

class FilteringAlgorithm;
class VirtualLineBias
{
public:
    VirtualLineBias(QString configFileIn,double deltaTimeIn);

public:
    double getSteeringMaxSpeed();
    double getSteeringMaxAcceration();
    int setDebugFlag(int flagIn);
    int setMaccrumIdealMovingLineAngle(double idealAngleIn);
    int setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn);
    int setVirtualLineEnableKpChange(bool isEnable);
    bool isInitialOk();
    int setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                          double kpOnlyRotationIn,double kpDistanceError_yIn);
    E_BIAS_TYPE getCalculateBiasType();
    int setCalculateBiasType(E_BIAS_TYPE calculateBiasTypeIn);
    int setCalculateBiasType_noDebug(E_BIAS_TYPE calculateBiasTypeIn);
    int setPositionFeedback(VehiclePosition vehiclePositionIn,VehicleVelocity currentVehicleVelocityIn);
    int setVehicleVelCommand(VehicleVelocity currentVehicleVelocityIn);
    int getBias(BiasControlInfo &controlInfoOut);

    int setVehicleDirection(bool isPositiveIn);
    int setPidStop(bool isStopIn);
    /**
     * @brief calculateBias
     * @param vehiclePositionCommandOld
     * @param vehiclePositionCommand
     * @param absoluteDistanceIn
     * @param absoluteAngleIn
     * @param deltaDistanceOut
     * @param deltaAngleOut
     * @return
     */
    int calculateBias(VehiclePosition vehiclePositionCommandOld,
                      VehiclePosition vehiclePositionCommand,
                      double absoluteDistanceIn, double absoluteAngleIn,
                      double &followDistanceError, double &followAngleError);
    int setActualFollowOffset(double distanceOffsetIn,double angleOffsetIn);
    int stopOutput();

    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角


private:
      int initialConfigFile(QString fileIn);
//    int calculateAbsoluteBias_moving(double &absoluteDistanceOut, double &absoluteAngleOut);
//    int calculateAbsoluteBias_end(double &absoluteDistanceOut, double &absoluteAngleOut);//not used
//    /**
//     * @brief calculateAbsoluteBias_end2 //几乎没有作用，弃用。轨迹跑完后，不再做这个函数的调整，依靠末端减速
//     * @param absoluteDistanceOut
//     * @param absoluteAngleOut
//     * @return
//     */
//    int calculateAbsoluteBias_end2(double &absoluteDistanceOut, double &absoluteAngleOut);
    int calculateMicroBias(VehiclePosition oldPos, VehiclePosition newPos, VehiclePosition actPos,
                           double &deltaDistanceOut, double &deltaAngleOut);
    /**
     * @brief calculateMicroBias_rotation 单独旋转时，刚性高的时候会导致偶尔因为卡顿导致震荡。做一个限制输出极限值处理。
     * @param oldPos_command
     * @param newPos_command
     * @param pos_feedback
     * @param pos_feedback_old
     * @param deltaDistanceOut
     * @param deltaAngleOut  角度偏移输出　rad
     * @param movingAngleBiasOut
     * @return
     */
    int calculateMicroBias_rotation(double deltaTimeIn, VehiclePosition oldPos_command, VehiclePosition newPos_command,
                                   VehiclePosition pos_feedback, VehiclePosition pos_feedback_old,
                           double &distanceFollowErrorOut, double &angleFollowErrorOut, double &deltaAngleOut,
                           double &rotateFeedwardOut, double &movingAngleBiasOut
                           , E_BIAS_TYPE lastBiasTypeIn);



    /**
     * @brief calculateMicroBias_stanley
     * @param oldPos_command 轨迹计算上次位置
     * @param newPos_command　轨迹计算本次位置
     * @param pos_feedback　车体反馈当前位置
     * @param deltaDistanceOut　轨迹方向速度控制偏差量
     * @param deltaAngleOut　轨迹旋转角度方向控制偏差量
     * @param movingAngleBiasOut 运动方向形成的角度，偏差误差报警用
     * @return
     */
    int calculateMicroBias_stanley(double deltaTimeIn, VehiclePosition oldPos_command, VehiclePosition newPos_command,
                                   VehiclePosition pos_feedback, VehiclePosition pos_feedback_old,
                                   double &distanceFollowErrorOut,double &angleFollowErrorOut,
                           double &deltaDistanceOut, double &deltaAngleOut, double &forward_xOut, double &forward_rOut,
                                   double &movingAngleBiasOut, E_BIAS_TYPE lastBiasTypeIn);
    int calculateMicroBias_maccrum(double deltaTimeIn,VehiclePosition oldPos_command, VehiclePosition newPos_command,
                                   VehiclePosition pos_feedback, VehiclePosition pos_feedback_old,
                                   double &distanceFollowErrorOut,double &angleFollowErrorOut,
                           double &deltaDistanceOut_x, double &deltaDistanceOut_y, double &deltaAngleOut,
                                   double &forward_xOut,double &forward_yOut, double &forward_rOut,
                                   double &movingAngleBiasOut, E_BIAS_TYPE lastBiasTypeIn);
    int calculateTargetBias(VehiclePosition targetPos,
                           double &deltaDistanceOut, double &deltaAngleOut);//NOT USE
    double transformToPi(double angleIn);
    int getVehicleMovingDirection(double &directionOut);//bug hualei need todo ,停止时没有方位

    int calculateNewKp(double old_ea, double old_ed_x,double old_ed_y,
                       VehicleVelocity velocityIn, double & new_ea , double &new_ed_x, double &new_ed_y);
    int translateErrorDistanceToVehicleDistance(double vehicleAngle, double movingAngle,
                                                double forwardErrorIn, double basideErrorIn, double &xErrorOut, double &yErrorOut);
    int translateErrorDistanceToVehicleDistance_new(double vehicleAngle, double movingAngle, double lagAngle,
                                                double lagDistance, double kpRatio, double &xErrorOut, double &yErrorOut);

    int renewPidPara();
    int checkPositionFeedbackForBias(VehiclePosition posFeedback,VehicleVelocity vehicleVelocity,
                           double &deltaDistanceOut_x, double &deltaDistanceOut_y, double &deltaAngleOut);

    int checkStartMoveOrientation(bool isMoveForward, double carOrientation, double idealMovingAngle);


private:
    QString configFile;
    double deltaDistance_x;//m ，车ｘ方向的距离差
    double deltaDistance_y;//m ，车ｙ方向的距离差
    double deltaAngle;//rad，车旋转方向的角度差
    double forward_x;//m/s ，前馈量
    double forward_y;//m/s ，前馈量
    double forward_r;//rad/s，前馈量
    double absoluteDistance;//m
    double absoluteAngle;//rad
    bool isPositiveDirection;//drive or rear
    bool isPidStop;//pid是否停止计算

    double lastMoveDirection;//最后一次轨迹计算的运行方向。
    VehicleVelocity currentVehicleVelocity_command;//实际传递进来的是手动模块自动命令速度，而不是反馈的速度。
    VehicleVelocity currentVehicleVelocity_feedback;
    VehiclePosition currentVehiclePosition;//反馈的位置
    VehiclePosition currentVehiclePosition_old;

    E_BIAS_TYPE calculateBiasType;
    E_BIAS_TYPE calculateBiasType_old_forDebug;
    bool isEnableKpChange;//是否运行ＰＩＤ参数随着速度变化。
    double kp_errorDistance_x;//控制调节系数,距离偏差(1:低速容易摆动，增益高了后摆动更加大，导致轨迹跟随不好。
    double kp_errorDistance_x_vmark;
    double kp_errorDistance_x_new;
    double kp_errorDistance_y;//全向轮用轨迹垂直距离误差控制调节系数,距离偏差(1:低速容易摆动，增益高了后摆动更加大，导致轨迹跟随不好。
    double kp_errorDistance_y_vmark;
    double kp_errorDistance_y_new;
    double kp_angleDistance;//控制调节系数，角度偏差(1:低速容易摆动，增益高了后摆动更加大，导致轨迹跟随不好。
    double kp_angleDistance_vmark;
    double kp_angleDistance_new;
    double kp_onlyRotation;//单独旋转时使用的增益。
    double kp_onlyRotation_vmark;
    double kp_onlyRotation_new;
    double kp_start_velocity;//增益变化的开始速度。
    double kp_end_velocity;//增益变化的结束速度。
    double kp_end_ratio;//增益变化结束比例系数。
    double kp_start_ratio;//增益变化开始比例系数。

    bool isInitialOkStatus;
    E_PATH_CONTROL_STRATEGY    pathControlStrategy;
    double idealAngle_vehicle;//ｒａｄ, 车的理想姿态角。
    int debugFlag;
    FilteringAlgorithm* filtering_moveR;//VMARK用差分结构
    FilteringAlgorithm* filtering_moveX;//暂停再启动有点攒动
    FilteringAlgorithm* filtering_moveY;//VMARK用麦克纳姆轮
    FilteringAlgorithm* filtering_feedforwardX;//防止突变。
    FilteringAlgorithm* filtering_feedforwardY;//防止突变。
    FilteringAlgorithm* filtering_feedforwardR;//走圆弧时会有突变，需要平滑处理。
    int vmark_filterCount;//vmark模式下，位置波动较大。
    int common_filterCount;
    double deltaTime;//周期时间
    double distanceFollowError;
    double angleFollowError;
    E_BIAS_TYPE   lastBiasType;
    E_BIAS_TYPE   lastBiasType_old;
    E_VEHICLE_TYPE vehicletype;
    double steeringMaxSpeed;
    double steeringMaxAcceration;
};

#endif // VIRTUALLINEBIAS_H
